//------------------------------------------------------------------------------
//  31 Band 1/3 Octave Equalizer For Pd
//
//  thirtyone~.c
//
//  Filters a signal through 31 evenly spaced
//  1/3 octave bandpass filters with gain
//
//  Created by Cooper Baker on 10/29/14.
//  Copyright (c) 2014 Cooper Baker. All rights reserved.
//------------------------------------------------------------------------------


//------------------------------------------------------------------------------
// headers
//------------------------------------------------------------------------------

// main header for pd
#include "m_pd.h"

// utility header for Pd Spectral Toolkit project
#include "utility.h"

// math header for fancy math
#include <math.h>

// stdio for debugging
#include <stdio.h>

// disable compiler warnings on windows
#ifdef NT
#pragma warning( disable : 4244 )
#pragma warning( disable : 4305 )
#endif

//------------------------------------------------------------------------------
// definitions
//------------------------------------------------------------------------------
#define NUM_BANDS 31

//------------------------------------------------------------------------------
// thirtyone_class - pointer to this object's definition
//------------------------------------------------------------------------------
static t_class* thirtyone_class;


//------------------------------------------------------------------------------
// thirtyone - data structure holding this object's data
//------------------------------------------------------------------------------
typedef struct thirtyone
{
    // this object - must always be first variable in struct
    t_object object;
   
    // needed for CLASS_MAINSIGNALIN macro call in thirtyone_tilde_setup
    t_float inlet_1;
   
    // coefficients for all filters
    double* b0;
    double* b1;
    double* b2;
    double* a1;
    double* a2;
   
    // sample memory for all filters
    double* x1;
    double* x2;
    double* y1;
    double* y2;
   
    // gains for all filters
    t_float* gains;
   
    // center frequencies for all filters
    t_float* centers;
   
    // sample rate
    t_float sr;
   
    // filter bandwidth
    t_float bw;
   
    // temp buffer
    t_float* temp;
   
} t_thirtyone;


//------------------------------------------------------------------------------
// function prototypes
//------------------------------------------------------------------------------
static void   thirtyone_messages    ( t_thirtyone* object, t_symbol* selector, t_int items, t_atom* list );
static void   thirtyone_calc_coeffs ( t_thirtyone* object );
static t_int* thirtyone_perform     ( t_int* io );
static void   thirtyone_dsp         ( t_thirtyone* object, t_signal **sig );
static void*  thirtyone_new         ( t_symbol* selector, t_int items, t_atom* list );
static void   thirtyone_free        ( t_thirtyone* object );
void          thirtyone_tilde_setup ( void );


//------------------------------------------------------------------------------
// thirtyone_calc_coeffs - calculates filter coefficients for each band
//
//      Adapted From:
//      Cookbook formulae for audio EQ biquad filter coefficients
//      By Robert Bristow-Johnson
//      http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
//
//------------------------------------------------------------------------------
static void thirtyone_calc_coeffs( t_thirtyone* object )
{
    double w0 = 0;
    double sin_w0 = 0;
    double alpha = 0;
    t_float bw = object->bw;
    t_float sr = object->sr;
    t_float a0 = 0;
   
    t_int i = 0;
   
    // calculate coefficients for each band
    for( i = 0 ; i < NUM_BANDS ; ++i )
    {
        // calculate intermediate values
        w0     = C_2_PI * object->centers[ i ] / sr;
        sin_w0 = sin( w0 );
        alpha  = sin_w0 * sinh( log( 2.0 ) / 2.0 * bw * w0 / sin_w0 );
       
        // calculate normalization coefficient
        a0 = ( 1.0 + alpha );

        // calculate filter coefficients for this band
        object->b0[ i ] = (  alpha           ) / a0;
        object->b1[ i ] = (  0.0             ) / a0;
        object->b2[ i ] = ( -alpha           ) / a0;
        object->a1[ i ] = ( -2.0 * cos( w0 ) ) / a0;
        object->a2[ i ] = (  1 - alpha       ) / a0;
    }
}


//------------------------------------------------------------------------------
// thirtyone_perform - the signal processing function of this object
//------------------------------------------------------------------------------
static t_int* thirtyone_perform( t_int* io )
{
    // store variables from dsp input/output array
    t_float*     in     = ( t_float*     )( io[ 1 ] );
    t_float*     out    = ( t_float*     )( io[ 2 ] );
    t_int        frames = ( t_int        )( io[ 3 ] );
    t_thirtyone* object = ( t_thirtyone* )( io[ 4 ] );
   
    // copy pointers to local variables
    t_float* temp = object->temp;
    double* b0    = object->b0;
    double* b1    = object->b1;
    double* b2    = object->b2;
    double* a1    = object->a1;
    double* a2    = object->a2;
    double* x1    = object->x1;
    double* x2    = object->x2;
    double* y1    = object->y1;
    double* y2    = object->y2;
    t_float* g    = object->gains;
   
    // filter iterator variable
    t_int i = -1;
 
    // signal vector iterator variable
    t_int n;

    // temporary sample variables
    double x = 0;
    double y = 0;
   
    // clear temp buffer
    memset( temp, 0, frames * sizeof( t_float ) );
   
    // iterate through the bands
    while( ++i < NUM_BANDS )
    {
        // reset vector iterator
        n = -1;
       
        // filter the input samples for each band
        while( ++n < frames )
        {
            // store input sample
            x = in[ n ];
           
            // direct form I biquad filter
            y = b0[i] * x + b1[i] * x1[i] + b2[i] * x2[i] - a1[i] * y1[i] - a2[i] * y2[i];

            // store previous input samples
            x2[i] = x1[i];
            x1[i] = x;
           
            // store previous output samples
            y2[i] = y1[i];
            y1[i] = y;
           
            // scale output by gain coefficient and mix into temp buffer
            temp[ n ] += y * g[ i ];
        }
    }

    // copy temp buffer of filtered bands to output
    memcpy( out, temp, frames * sizeof( t_float ) );

    // return the dsp input/output array address plus one more than its size
    // to provide a pointer to the next perform function in pd's call list
    return &( io[ 5 ] );
}


//------------------------------------------------------------------------------
// thirtyone_dsp - installs this object's dsp function in pd's callback list
//------------------------------------------------------------------------------
static void thirtyone_dsp( t_thirtyone* object, t_signal **sig )
{
    // store sample rate
    object->sr = sig[ 0 ]->s_sr;
   
    // allocate temporary audio buffer
    object->temp = realloc( object->temp, sig[ 0 ]->s_n * sizeof( t_float ) );
   
    // calculate coefficients
    thirtyone_calc_coeffs( object );
   
    // dsp_add arguments
    //--------------------------------------------------------------------------
    // perform routine
    // number of passed parameters
    // inlet sample vector
    // outlet sample vector
    // sample frames to process (vector size)
    // object pointer
    dsp_add( thirtyone_perform, 4, sig[ 0 ]->s_vec, sig[ 1 ]->s_vec, sig[ 0 ]->s_n, object );
}


//------------------------------------------------------------------------------
// thirtyone_messages - parses messages
//------------------------------------------------------------------------------
static void thirtyone_messages( t_thirtyone* object, t_symbol* selector, t_int items, t_atom* list )
{
    if( StringMatch( selector->s_name, "list" ) )
    {
        if( items == 2 )
        {
            if( ( list[ 0 ].a_type == A_FLOAT ) && ( list[ 1 ].a_type == A_FLOAT ) )
            {
                t_int   index   = list[ 0 ].a_w.w_float;
                t_float db_gain = list[ 1 ].a_w.w_float;
               
                index = Clip( index, 0, NUM_BANDS - 1 );
               
                object->gains[ index ] = DbToA( db_gain );
            }
            else
            {
                pd_error( object, "thirtyone~: invalid message" );
            }
        }
        else if( items == NUM_BANDS )
        {
            int valid = 1;
            int i = 0;

            for( i = 0 ; i < NUM_BANDS ; ++i )
            {
                if( list[ i ].a_type != A_FLOAT )
                {
                    valid = 0;
                    break;
                }
            }
           
            if( valid )
            {
                t_float db_gain = 0;
               
                for( i = 0 ; i < NUM_BANDS ; ++i )
                {
                    db_gain = list[ i ].a_w.w_float;
                    object->gains[ i ] = DbToA( db_gain );
                }
            }
            else
            {
                pd_error( object, "thirtyone~: invalid message" );
            }
        }
        else
        {
            pd_error( object, "thirtyone~: invalid message" );
        }

    }
    else if( StringMatch( selector->s_name, "reset" ) )
    {
        int i = 0;
       
        for( i = 0 ; i < NUM_BANDS ; ++i )
        {
            object->gains[ i ] = 1;
        }
    }
    else
    {
        pd_error( object, "thirtyone~: invalid message" );
    }
}


//------------------------------------------------------------------------------
// thirtyone_new - instantiates a copy of this object in pd
//------------------------------------------------------------------------------
static void* thirtyone_new( t_symbol* selector, t_int items, t_atom* list )
{
    // create a pointer to this object
    t_thirtyone* object = ( t_thirtyone* )pd_new( thirtyone_class );
   
    // create a new signal outlet for this object
    outlet_new( &object->object, gensym( "signal" ) );
   
    // set bandwidth of filters
    object->bw = 1.0 / 3.0;
   
    // set pointers to null
    object->b0      = NULL;
    object->b1      = NULL;
    object->b2      = NULL;
    object->a1      = NULL;
    object->a2      = NULL;
    object->gains   = NULL;
    object->centers = NULL;
    object->temp    = NULL;

    // allocate memory for filter parameters
    object->b0      = malloc( NUM_BANDS * sizeof( double  ) );
    object->b1      = malloc( NUM_BANDS * sizeof( double  ) );
    object->b2      = malloc( NUM_BANDS * sizeof( double  ) );
    object->a1      = malloc( NUM_BANDS * sizeof( double  ) );
    object->a2      = malloc( NUM_BANDS * sizeof( double  ) );
    object->x1      = malloc( NUM_BANDS * sizeof( double  ) );
    object->x2      = malloc( NUM_BANDS * sizeof( double  ) );
    object->y1      = malloc( NUM_BANDS * sizeof( double  ) );
    object->y2      = malloc( NUM_BANDS * sizeof( double  ) );
    object->gains   = malloc( NUM_BANDS * sizeof( t_float ) );
    object->centers = malloc( NUM_BANDS * sizeof( t_float ) );
   
    // set memory of filters to zero
    memset( object->x1, 0, NUM_BANDS * sizeof( double ) );
    memset( object->x2, 0, NUM_BANDS * sizeof( double ) );
    memset( object->y1, 0, NUM_BANDS * sizeof( double ) );
    memset( object->y2, 0, NUM_BANDS * sizeof( double ) );

    // initialize centers and gains
    t_int i = 0;
    t_float freq = 20;

    for( i = 0 ; i < NUM_BANDS ; ++i )
    {
        // printf( "%5.1f\n", freq );
       
        // 1/3 octave spacing
        object->centers[ i ] = freq;
        freq = freq * powf( 2.0, ( 1.0 / 3.0 ) );
       
        // unity gain ( 0 dB )
        object->gains[ i ] = 1;
    }
   
    // parse creation arguments
    if( items == NUM_BANDS )
    {
        int valid = 1;
        int i = 0;
       
        for( i = 0 ; i < NUM_BANDS ; ++i )
        {
            if( list[ i ].a_type != A_FLOAT )
            {
                valid = 0;
                break;
            }
        }

        if( valid )
        {
            t_float db_gain = 0;
           
            for( i = 0 ; i < NUM_BANDS ; ++i )
            {
                db_gain = list[ i ].a_w.w_float;
                object->gains[ i ] = DbToA( db_gain );
            }
        }
        else
        {
            pd_error( object, "thirtyone~: invalid initialization arguments" );
        }
    }
   
    return object;
}


//------------------------------------------------------------------------------
// thirtyone_free - cleans up memory allocated by this object
//------------------------------------------------------------------------------
static void thirtyone_free( t_thirtyone* object )
{
    // if memory is allocated
    if( object->b0 )
    {
        // free the memory
        free( object->b0 );

        // set memory pointer to null
        object->b0 = NULL;
    }
   
    // . . .
    if( object->b1 )
    {
        free( object->b1 );
        object->b1 = NULL;
    }

    if( object->b2 )
    {
        free( object->b2 );
        object->b2 = NULL;
    }

    if( object->a1 )
    {
        free( object->a1 );
        object->a1 = NULL;
    }

    if( object->a2 )
    {
        free( object->a2 );
        object->a2 = NULL;
    }
   
    if( object->x1 )
    {
        free( object->x1 );
        object->x1 = NULL;
    }
   
    if( object->x2 )
    {
        free( object->x2 );
        object->x2 = NULL;
    }
   
    if( object->y1 )
    {
        free( object->y1 );
        object->y1 = NULL;
    }
   
    if( object->y2 )
    {
        free( object->y2 );
        object->y2 = NULL;
    }
   
    if( object->gains )
    {
        free( object->gains );
        object->gains = NULL;
    }
   
    if( object->centers )
    {
        free( object->centers );
        object->centers = NULL;
    }
   
    if( object->temp )
    {
        free( object->temp );
        object->temp = NULL;
    }
}


//------------------------------------------------------------------------------
// thirtyone_tilde_setup - describes the attributes of this object to pd so it may be properly instantiated
// (must always be named with _tilde replacing ~ in the object name)
//------------------------------------------------------------------------------
void thirtyone_tilde_setup( void )
{
    // creates an instance of this object and describes it to pd
    thirtyone_class = class_new( gensym( "thirtyone~" ), ( t_newmethod )thirtyone_new, ( t_method )thirtyone_free, sizeof( t_thirtyone ), 0, A_GIMME, 0 );
   
    // declares leftmost inlet as a signal inlet
    CLASS_MAINSIGNALIN( thirtyone_class, t_thirtyone, inlet_1 );
   
    // installs thirtyone_dsp so that it will be called when dsp is turned on
    class_addmethod( thirtyone_class, ( t_method )thirtyone_dsp, gensym( "dsp" ), 0 );
   
    // installs thirtyone_set to respond to "set ___" messages
    class_addmethod( thirtyone_class, ( t_method )thirtyone_messages, gensym( "anything" ), A_GIMME, 0 );
   
    // announce this object in the pd console
    Announce( "thirtyone~: 31 band 1/3 octave equalizer - v1.0" );
}


//------------------------------------------------------------------------------
// EOF
//------------------------------------------------------------------------------